import cv2
from numpy import *
import numpy as np
import math
object_3d_points = np.array(([2750, 890, 0],
                            [3500, 450, 0],
                            [3620, -50,  0],
                            [2790, -240, 0]), dtype=np.double)
object_2d_point = np.array(([558, 259],
                            [629, 212],
                            [693, 209],
                            [744, 260]), dtype=np.double)
camera_matrix = np.array(([328.47364883, 0, 629.0359318],
                         [0, 328.4303046, 340.13011656],
                         [0, 0, 1.0]), dtype=np.double)

# dist_coefs = np.array([0.06147452, 0.03188344, -0.00678974, 0, 0], dtype=np.double)
dist_coefs = np.array([0.06147452, 0.03188344, 0, 0, -0.00678974], dtype=np.double)
# 求解相机位姿
found, rvec, tvec = cv2.solvePnP(object_3d_points, object_2d_point, camera_matrix, dist_coefs)
# print(rvec, tvec)
rotM = cv2.Rodrigues(rvec)[0]
camera_postion = -np.matrix(rotM).T * np.matrix(tvec)
# print(camera_postion.T)
# 验证根据博客http://www.cnblogs.com/singlex/p/pose_estimation_1.html提供方法求解相机位姿
# 计算相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。旋转顺序z,y,x
thetaZ = math.atan2(rotM[1, 0], rotM[0, 0])*180.0/math.pi
thetaY = math.atan2(-1.0*rotM[2, 0], math.sqrt(rotM[2, 1]**2 + rotM[2, 2]**2))*180.0/math.pi
thetaX = math.atan2(rotM[2, 1], rotM[2, 2])*180.0/math.pi
# 相机坐标系下值
x = tvec[0]
y = tvec[1]
z = tvec[2]


# 进行三次旋转
def RotateByZ(Cx, Cy, thetaZ):
    rz = thetaZ*math.pi/180.0
    outX = math.cos(rz)*Cx - math.sin(rz)*Cy
    outY = math.sin(rz)*Cx + math.cos(rz)*Cy
    return outX, outY


def RotateByY(Cx, Cz, thetaY):
    ry = thetaY*math.pi/180.0
    outZ = math.cos(ry)*Cz - math.sin(ry)*Cx
    outX = math.sin(ry)*Cz + math.cos(ry)*Cx
    return outX, outZ


def RotateByX(Cy, Cz, thetaX):
    rx = thetaX*math.pi/180.0
    outY = math.cos(rx)*Cy - math.sin(rx)*Cz
    outZ = math.sin(rx)*Cy + math.cos(rx)*Cz
    return outY, outZ


(x, y) = RotateByZ(x, y, -1.0*thetaZ)
(x, z) = RotateByY(x, z, -1.0*thetaY)
(y, z) = RotateByX(y, z, -1.0*thetaX)
Cx = x*-1
Cy = y*-1
Cz = z*-1
# 输出相机位置
print(Cx, Cy, Cz)
# 输出相机旋转角
print(thetaX, thetaY, thetaZ)
# 对第五个点进行验证
Out_matrix = np.concatenate((rotM, tvec), axis=1)
pixel = np.dot(camera_matrix, Out_matrix)
pixel1 = np.dot(pixel, np.array([4120, 270, 0, 1], dtype=np.double))
pixel2 = pixel1/pixel1[2]
# print(pixel1[2])
print(pixel2)

# 计算参数S
temmat = mat(rotM).I * mat(camera_matrix).I * mat([754, 217, 1]).T
temmat2 = mat(rotM).I * mat(tvec)
# print(temmat, temmat2)
s = temmat2[2]
s = s/temmat[2]
# print(s)


# 计算世界坐标
wcpoint = mat(rotM).I * (s[0, 0] * mat(camera_matrix).I * mat([754, 217, 1]).T - mat(tvec))
print(wcpoint)
